#include "./BSP/SYSTEM/system.h"
#include "./BSP/SOFT_I2C/soft_i2c.h"
#include "./BSP/GW_TRACK/gw_track.h"
#include "./BSP/DEBUG/debug.h"

static const float weights[8] = {-4.5f, -5.0f, -6.5f, -2.5f, 2.5f, 6.5f, 5.0f, 4.5f}; // 8个灰度值对应的权值

static uint8_t digital = 0x00;
#if CFG_GW_TRACK_MODE == 2
const GPIO_Regs *gw_track_gpio_regs[CFG_GW_TRACK_GPIO_NUM] = {CFG_GW_TRACK_GPIO_PORT};
const uint32_t gw_track_pin[CFG_GW_TRACK_GPIO_NUM] = {CFG_GW_TRACK_GPIO_PIN};
#endif
/**
 * @brief       初始化循迹传感器
 * @param       无
 * @retval      无
 */
void track_init(void)
{
#if CFG_GW_TRACK_MODE == 1
    soft_i2c_init(CFG_GW_TRACK_SOFT_IIC_ID);
#elif CFG_GW_TRACK_MODE == 2

#endif
}

/**
 * @brief       读取循迹传感器数据
 * @param       无
 * @retval      无
 */
void track_read_sensors(void)
{
    uint8_t data = 0;

#if CFG_GW_TRACK_MODE == 1
    soft_i2c_read_data(CFG_GW_TRACK_SOFT_IIC_ID, GW_GRAY_ADDR_DEF << 1, GW_GRAY_DIGITAL_MODE, &data, 1); // 读取传感器数据
#elif CFG_GW_TRACK_MODE == 2
    for (uint8_t i = 0; i < CFG_GW_TRACK_GPIO_NUM; i++)
    {
        // data |= (DL_GPIO_readPins((GPIO_Regs *)gw_track_gpio_regs[i], gw_track_pin[i]) & gw_track_pin[i]) << i; // 读取传感器数据
        data |= (((DL_GPIO_readPins((GPIO_Regs *)gw_track_gpio_regs[i], gw_track_pin[i]) & gw_track_pin[i]) > 0) ? 1 : 0) << i;
    }
#endif

    digital = ~data; // 取反数据，0表示黑线，1表示白线
}

/**
 * @brief       获取传感器数据
 * @param       无
 * @retval      传感器数据
 */
uint8_t track_get_sensor_data(void)
{
    return digital; // 返回传感器数据
}

/**
 * @brief       获取循迹误差值
 * @param        sensor_data: 传感器数据
 * @retval      循迹误差值
 */
float track_get_line_position_error(uint8_t sensor_data)
{
    float error = 0.0f;
    uint8_t black_count = 0;
    for (uint8_t i = 0; i < 8; i++)
    {
        if (sensor_data & (1 << i))
        {
            error += weights[i];
            black_count++;
        }
    }
    if (black_count > 0)
    {
        error /= black_count; // 计算平均误差
    }
    return error; // 返回循迹误差值
}

/**
 * @brief       显示循迹传感器数据
 * @param       无
 * @retval      无
 */
void track_oled_show(void)
{
#include "./BSP/OLED/OLED.h"
    uint8_t cnt = track_get_sensor_data();
    OLED_Printf(0, 8 * 7, OLED_6X8, "BIT:%d %d %d %d %d %d %d %d        ", cnt & 0x01 ? 1 : 0, cnt & 0x02 ? 1 : 0, cnt & 0x04 ? 1 : 0, cnt & 0x08 ? 1 : 0, cnt & 0x10 ? 1 : 0, cnt & 0x20 ? 1 : 0, cnt & 0x40 ? 1 : 0, cnt & 0x80 ? 1 : 0);
}
